'''
Created on Nov 30, 2009

@author: Peter
'''
from numpy import array, eye, linalg
from math import cos, sin, sqrt
import math
def getZRotationMatrix(angleZ):
    """Returns a rotation matrix around the Z-Axis by angleZ degrees"""
    angle = angleZ * math.pi / 180
    return array([[cos(angle), sin(angle), 0],
                  [-sin(angle), cos(angle), 0],
                  [0, 0, 1]])

def getXRotationMatrix(angleX):
    angle = angleX * math.pi / 180
    return array([[1, 0, 0],
                  [0, cos(angle), sin(angle)],
                  [0, -sin(angle), cos(angle)]])

def getYRotationMatrix(angleY):
    angle = angleY * math.pi / 180
    return array([[cos(angle), 0, -sin(angle)],
                  [0, 1, 0],
                  [sin(angle), 0, cos(angle)]])

def getTrMatrix(tr):
    t = eye(4)
    t[:3, 3] = tr
    return t

def getAxisRotationMatrix(axis, angle):
    c = cos(angle)
    s = sin(angle)
    t = 1 - c
    X = axis[0]
    Y = axis[1]
    Z = axis[2]
    matx = array([[t * X * X + c, t * X * Y + s * Z, t * X * Z - s * Y, 0],
                  [t * X * Y - s * Z, t * Y * Y + c, t * Y * Z + s * X, 0],
                  [t * X * Z + s * Y, t * Y * Z - s * X, t * Z * Z + c, 0],
                  [0, 0, 0, 1]])
    return matx

def unitV(vector):
    normVal = linalg.norm(vector)
    if normVal - 1 < 1e-6:
        return vector
    return vector / normVal

def rpyFromRotMatrix(rotMatrix):

    yaw = math.atan2(rotMatrix[2][1], rotMatrix[2][2])   # rotate about x0
    pitch = math.atan2(-rotMatrix[2][0], sqrt(1.00000001 - rotMatrix[2][0] * rotMatrix[2][0])) # rotate about y0
    roll = math.atan2(rotMatrix[1][0], rotMatrix[0][0]) # rotate about z0

    #print "yaw ", yaw * 180/pi
    #print "pitch a ", pitch * 180/pi
    #print "roll a ", roll * 180/pi
    #print
    return array([roll, pitch, yaw])


def rotMatrixFromRpy(rollRad, pitchRad, yawRad):

    cr = cos(rollRad)
    sr = sin(rollRad)
    cp = cos(pitchRad)
    sp = sin(pitchRad)
    cy = cos(yawRad)
    sy = sin(yawRad)

    R00 = cr * cp
    R01 = -sr * cy + cr * sp * sy
    R02 = sr * sy + cr * sp * cy

    R10 = sr * cp
    R11 = cr * cy + sr * sp * sy
    R12 = -cr * sy + sr * sp * cy

    R20 = -sp
    R21 = cp * sy
    R22 = cp * cy

    return array([[R00, R01, R02], [R10, R11, R12], [R20, R21, R22]])
'''
Created on Dec 14, 2009

@author: grasping
'''
def getCylinder(heightInM,env):
    createCylinderXML(heightInM)
    cyl = env.ReadKinBodyXMLFile("/tmp/cylinder.xml")
    return cyl

def getAxisMarker(env):
    createAxisMarker()
    axes = env.ReadKinBodyXMLFile("/tmp/axis.xml")
    return axes

def createCylinderXML(heightInM):
    fp = file("/tmp/cylinder.xml","w")
    fp.write('<?xml version="1.0" encoding="utf-8"?>')
    fp.write("\n")
    fp.write('<KinBody name="Cylinder">')
    fp.write('<Translation>0.98 0 -0.5710</Translation>')
    fp.write('<Body name="Body1" type="Dynamic">')
    fp.write('<Geom type="cylinder">')
    fp.write('<DiffuseColor>0.8 0 0</DiffuseColor>')
    fp.write('<RotationAxis>1 0 0 90</RotationAxis>')
    fp.write('<Translation>0 0 0.052</Translation>')
    fp.write('<Radius>0.2</Radius>')
    fp.write('<Height>'+str(heightInM)+'</Height>')
    fp.write('</Geom>')
    fp.write('</Body>')
    fp.write('</KinBody>')
    fp.close()

def createAxisMarker():
    fp = file("/tmp/axis.xml",'w')
    fp.write('<?xml version="1.0" encoding="utf-8"?>')
    fp.write("\n")
    fp.write('<KinBody name="Axis">')
    fp.write('<Body name="body" type="dynamic">')
    fp.write('<Geom type="trimesh">')
    fp.write('<data>models/axes/cross.iv</data>')
    fp.write('<render>models/axes/cross.iv</render>')
    fp.write('</Geom>')
    fp.write('</Body>')
    fp.write('</KinBody>')
    fp.close()

def createContactMarker(colorval):
    fp = file("/tmp/contact.xml",'w')
    fp.write('<?xml version="1.0" encoding="utf-8"?>')
    fp.write("\n")
    fp.write('<KinBody name="Axis">')
    fp.write('<Body name="contact" type="dynamic">')
    fp.write('<Geom type="sphere">')
    fp.write('<DiffuseColor>'+colorval+'</DiffuseColor>')
    fp.write('<Radius>0.005</Radius>')
    fp.write('</Geom>')
    fp.write('</Body>')
    fp.write('</KinBody>')
    fp.close()

def getContactMarker(env, colorval):
    createContactMarker(colorval)
    cm = env.ReadKinBodyXML("/tmp/contact.xml")
    return cm

def get_xml_ball(point):
    """Given a 3d point, creates an XML string representing a ball in OpenRAVE.
    
    Keyword arguments:
    point -- a list-like object containing a 3d point, (x,y,z)
    
    Usage:
    env = orp.Environment()
    ball_str = get_xml_ball((0,0,0))
    ball = env.GetKinBodyXMLData(ball_str)
    """

    string = '<?xml version="1.0" encoding="utf-8"?>'
    string += '<KinBody name="TestChain">'
    string += '<Body name="Body1" type="dynamic">'
    string += '<Geom type="sphere">'
    string += '<Radius>0.003</Radius>'
    string += '<Translation>' + ' '.join(str(p) for p in point) + '</Translation>'
    string += '</Geom>'
    string += '</Body>'
    string += '</KinBody>'
    return string